#include "slepbase.h"
#include "epph.h"



void mtLeastR(slep_matrix* A, slep_vector* y, double z, OPTS* opts,
			  slep_matrix* X, slep_vector* funVal, slep_vector* valueL){
	int *ind, k, i;
	double q, lambda, q_bar, lambda_max;
	slep_matrix *ATy;
	slep_vector *Ax;
	assert(A->m == y->dim);
	assert(z>=0) ;
	sll_opts(opts);
	assert(hasGot(opts,SlepOPT_indices));
	ind = opts->ind;
	k = opts->ind_size - 1;
	if(hasGot(opts,SlepOPT_q)){
		assert(opts->q > 1.0);
		q =  opts->q;
	}else
		q = opts->q = 2.0;
	
	initNormalization(A,opts);
	ATy = slepAllocDenseMatrix(A->n, k);
	slep_zerom(ATy);

	{
		slep_matrix A_part = *A;
		slep_vector ATy_i ;
		slep_vector y_i;
		ATy_i.dim = A->n; 
		ATy_i.inc = 1;
		ATy_i.sparsity = SlepDense;
		y_i.inc = 1;
		y_i.sparsity = SlepDense;
		for(i=0; i<k; i++){
			A_part.data.mtrx = &(A->data.mtrx[ind[i]]);
			A_part.m = ind[i+1] - ind[i];
			ATy_i.data.vctr = &(ATy->data.mtrx[i*ATy->ld]);
			y_i.data.vctr = &(y->data.vctr[ind[i]]);
			y_i.dim = ind[i+1] - ind[i];
			slep_daAxpby(SlepTranspose,1.0,&A_part, &y_i, 0.0, &ATy_i);
		}
	}

	if(opts->rFlag == 0)
		lambda = z;
	else{
		slep_vector ATy__i;
		double temp;
		assert( z>=0 && z<=1 );
		if(q == 1.0)
			q_bar = 1e30;
		else if( q>1e6)
			q_bar = 1.0;
		else
			q_bar = q/(q-1.0);
		lambda_max = 0.0;
		ATy__i.dim = ATy->n;
		ATy__i.sparsity = SlepDense;
		ATy__i.inc = ATy->ld;
		for(i=0; i<ATy->m; i++){
			ATy__i.data.vctr = &(ATy->data.mtrx[i]);
			temp = slep_dpnrm(&ATy__i,q_bar);
			lambda_max = SLEPMAX(lambda_max,temp);
		}
		lambda = z * lambda_max;
	}

	if( opts->init == 2)
		slep_zerom(X);
	else{
		if( hasGot(opts,SlepOPT_X0)){
			slep_dmcopy(opts->X0, X);
			assert(X->m == A->n && X->n == k );
		}else
			slep_dmcopy(ATy,X);
	}

	Ax = slepAllocDenseVector(y->dim);
	slep_zerov(Ax);

	{
		slep_matrix A_i = *A;
		slep_vector Ax_i= *Ax, x_i;
		x_i.dim = X->m;
		x_i.inc = 1;
		x_i.sparsity = SlepDense;
		for(i=0; i < k ; i++){
			A_i.m = ind[i+1] - ind[i];
			Ax_i.dim = ind[i+1] - ind[i];
			A_i.data.mtrx = &(A->data.mtrx[ind[i]]);
			Ax_i.data.vctr = &(Ax->data.vctr[ind[i]]);
			x_i.data.vctr = &(X->data.mtrx[i*(X->ld)]);
			slep_daAxpby(SlepNoTranspose,1.0, &A_i, &x_i, 0.0, &Ax_i);
		}
	}

	if( opts->init == 0){
		double x_norm = 0.0;
		// TODO multiply by ratio and call initFactor
	}

	if( opts->mFlag==0 && opts->lFlag==0){
		int bFlag = 0, iterStep;
		double L=1.0, alpha, alphap, beta;
		slep_matrix *Xp, *XXp, *S, *ATAs, *G, *V;
		slep_vector *Axp, *As, *Axy, *Av;
		double l_sum , r_sum;

		Xp = slepCopyToNewMatrix(X);
		Axp = slepCopyToNewVector(Ax);
		XXp = slepAllocDenseMatrix(X->m, X->n);
		S = slepAllocDenseMatrix(X->m, X->n);
		G = slepAllocDenseMatrix(X->m, X->n);
		V = slepAllocDenseMatrix(X->m, X->n);
		ATAs = slepAllocDenseMatrix(X->m, X->n);
		As = slepAllocDenseVector(Ax->dim);
		Av = slepAllocDenseVector(Ax->dim);
		Axy = slepAllocDenseVector(Ax->dim);
		slep_zerom(XXp);
		alphap = 0.0;
		alpha = 1.0;
		for(iterStep = 0; iterStep < opts->maxIter ; ++iterStep){
			beta = ( alphap - 1.0)/ alpha;
			slep_dmcopy(X,S);
			slep_daXpbY(SlepNoTranspose,beta,XXp,1.0,S);
			slep_dvcopy(Ax,As);
			slep_daxpby(-beta,Axp,1.0+beta,As);
			// ATAs
			{
				slep_matrix A_part = *A;
				slep_vector ATAs_i ;
				slep_vector As_i;
				ATAs_i.dim = A->n; 
				ATAs_i.inc = 1;
				ATAs_i.sparsity = SlepDense;
				As_i.inc = 1;
				As_i.sparsity = SlepDense;
				for(i=0; i<k; i++){
					A_part.data.mtrx = &(A->data.mtrx[ind[i]]);
					A_part.m = ind[i+1] - ind[i];
					ATAs_i.data.vctr = &(ATAs->data.mtrx[i*ATAs->ld]);
					As_i.data.vctr = &(As->data.vctr[ind[i]]);
					As_i.dim = ind[i+1] - ind[i];
					slep_daAxpby(SlepTranspose,1.0,&A_part, &As_i, 0.0, &ATAs_i);
				}
			}

			// g=ATAs-ATy;
			slep_dmcopy(ATAs,G);
			slep_daXpbY(SlepNoTranspose,-1.0, ATy, 1.0, G);

			slep_dmcopy(X, Xp);
			slep_dvcopy(Ax, Axp);
			for(;;){
				// v = s - g/L;
				slep_dmcopy(S,V);
				slep_daXpbY(SlepNoTranspose,-1.0/L, G, 1.0, V);

				eppMatrix(X->data.mtrx,V->data.mtrx, X->m, X->n, lambda/L, q);
				//v=x-s;
				slep_dmcopy(X,V);
				slep_daXpbY(SlepNoTranspose, -1.0, S, 1.0, V);

				{
					slep_matrix A_i = *A;
					slep_vector Ax_i= *Ax, x_i;
					x_i.dim = X->m;
					x_i.inc = 1;
					x_i.sparsity = SlepDense;
					for(i=0; i < k ; i++){
						A_i.m = ind[i+1] - ind[i];
						Ax_i.dim = ind[i+1] - ind[i];
						A_i.data.mtrx = &(A->data.mtrx[ind[i]]);
						Ax_i.data.vctr = &(Ax->data.vctr[ind[i]]);
						x_i.data.vctr = &(X->data.mtrx[i*(X->ld)]);
						slep_daAxpby(SlepNoTranspose,1.0, &A_i, &x_i, 0.0, &Ax_i);
					}
				}
				// Av = Ax - As;
				slep_dvcopy(Ax, Av);
				slep_daxpby( -1.0, As, 1.0, Av);
				r_sum = slep_dfrbnrm(V);
				r_sum = r_sum * r_sum;
				l_sum = slep_ddot(Av,Av);

				if( r_sum <= 1e-20){
					bFlag = 1;
					break;
				}
				if( l_sum <= r_sum * L)
					break;
				else
					L = SLEPMAX(2*L, l_sum / r_sum);
			}
			alphap = alpha;
			alpha = ( 1.0+ sqrt(4*alpha*alpha+1.0))/2.0;
			valueL->data.vctr[iterStep] = L;
			slep_dmcopy(X,XXp);
			slep_daXpbY(SlepNoTranspose, -1.0, Xp, 1.0, XXp);
			slep_dvcopy(Ax, Axy);
			slep_daxpby(-1.0, y, 1.0, Axy);

			funVal->data.vctr[iterStep] = slep_ddot(Axy,Axy)/2.0;

			{
				slep_vector x_i;
				x_i.dim = X->n;
				x_i.inc = X->ld;
				x_i.sparsity = SlepDense;
				for(i=0; i < X->m ; i++){
					x_i.data.vctr = &(X->data.mtrx[i]);
					funVal->data.vctr[iterStep] += lambda * slep_dpnrm(&x_i,q);
				}
			}

			if(bFlag)
				break;
			if( opts->tFlag == 0){
				if( iterStep >= 2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dfrbnrm(XXp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dfrbnrm(XXp) <= (opts->tol) * max(slep_dfrbnrm(Xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeMatrix(Xp);
		slepFreeMatrix(XXp);
		slepFreeMatrix(S);
		slepFreeMatrix(ATAs);
		slepFreeMatrix(G);
		slepFreeMatrix(V);
		slepFreeVector(Axp);
		slepFreeVector(As);
		slepFreeVector(Axy);
		slepFreeVector(Av);
		funVal->dim = iterStep;
		valueL->dim = iterStep;

	}


	if( opts->mFlag==1 && opts->lFlag==0 && SLEPDoubleApproxEquals(opts->q,2.0) ){
		int bFlag = 0, iterStep;
		double L=1.0, alpha, alphap, beta;
		slep_matrix *Xp, *XXp, *S, *ATAs, *G, *U, *V;
		slep_vector *Axp, *As, *Axy, *Av;
		slep_vector *t, *tp, *s_t, *v;
		double l_sum , r_sum;

		Xp = slepCopyToNewMatrix(X);
		Axp = slepCopyToNewVector(Ax);
		XXp = slepAllocDenseMatrix(X->m, X->n);
		S = slepAllocDenseMatrix(X->m, X->n);
		G = slepAllocDenseMatrix(X->m, X->n);
		V = slepAllocDenseMatrix(X->m, X->n);		
		U = slepAllocDenseMatrix(X->m, X->n);
		ATAs = slepAllocDenseMatrix(X->m, X->n);
		As = slepAllocDenseVector(Ax->dim);
		Av = slepAllocDenseVector(Ax->dim);
		Axy = slepAllocDenseVector(Ax->dim);

		v = slepAllocDenseVector(X->m);
		t = slepAllocDenseVector(X->m);
		tp = slepAllocDenseVector(X->m);
		s_t = slepAllocDenseVector(X->m);

		{
			slep_vector x__i=*t;
			x__i.dim = X->n;
			x__i.inc = X->ld;
			for(i=0; i< X->m; ++i){
				x__i.data.vctr = &(X->data.mtrx[i]);
				tp->data.vctr[i] = t->data.vctr[i] = slep_dnrm2(&x__i);
			}
		}

		slep_zerom(XXp);
		alphap = 0.0;
		alpha = 1.0;
		for(iterStep = 0; iterStep < opts->maxIter ; ++iterStep){
			beta = ( alphap - 1.0)/ alpha;
			slep_dmcopy(X,S);
			slep_daXpbY(SlepNoTranspose,beta,XXp,1.0,S);
			slep_dvcopy(t,s_t);
			slep_daxpby(-beta,tp, 1.0+beta, s_t);

			slep_dvcopy(Ax,As);
			slep_daxpby(-beta,Axp,1.0+beta,As);
			// ATAs
			{
				slep_matrix A_part = *A;
				slep_vector ATAs_i ;
				slep_vector As_i;
				ATAs_i.dim = A->n; 
				ATAs_i.inc = 1;
				ATAs_i.sparsity = SlepDense;
				As_i.inc = 1;
				As_i.sparsity = SlepDense;
				for(i=0; i<k; i++){
					A_part.data.mtrx = &(A->data.mtrx[ind[i]]);
					A_part.m = ind[i+1] - ind[i];
					ATAs_i.data.vctr = &(ATAs->data.mtrx[i*ATAs->ld]);
					As_i.data.vctr = &(As->data.vctr[ind[i]]);
					As_i.dim = ind[i+1] - ind[i];
					slep_daAxpby(SlepTranspose,1.0,&A_part, &As_i, 0.0, &ATAs_i);
				}
			}

			// g=ATAs-ATy;
			slep_dmcopy(ATAs,G);
			slep_daXpbY(SlepNoTranspose,-1.0, ATy, 1.0, G);

			slep_dmcopy(X, Xp);
			slep_dvcopy(Ax, Axp);
			slep_dvcopy(t,tp);
			for(;;){
				// u = s - g/L;
				slep_dmcopy(S,U);
				slep_daXpbY(SlepNoTranspose,-1.0/L, G, 1.0, U);

				// v = s_t - lambda / L;
				DMEMSET(v->data.vctr, -lambda/L,v->dim,i);
				slep_daxpby(1.0,s_t,1.0,v);

				ep21R(X->data.mtrx, t->data.vctr, U->data.mtrx, v->data.vctr, X->m, X->n);

				//V=x-s; different V here :)
				slep_dmcopy(X,V);
				slep_daXpbY(SlepNoTranspose, -1.0, S, 1.0, V);

				{
					slep_matrix A_i = *A;
					slep_vector Ax_i= *Ax, x_i;
					x_i.dim = X->m;
					x_i.inc = 1;
					x_i.sparsity = SlepDense;
					for(i=0; i < k ; i++){
						A_i.m = ind[i+1] - ind[i];
						Ax_i.dim = ind[i+1] - ind[i];
						A_i.data.mtrx = &(A->data.mtrx[ind[i]]);
						Ax_i.data.vctr = &(Ax->data.vctr[ind[i]]);
						x_i.data.vctr = &(X->data.mtrx[i*(X->ld)]);
						slep_daAxpby(SlepNoTranspose,1.0, &A_i, &x_i, 0.0, &Ax_i);
					}
				}
				// Av = Ax - As;
				slep_dvcopy(Ax, Av);
				slep_daxpby( -1.0, As, 1.0, Av);
				r_sum = slep_dfrbnrm(V);
				r_sum = r_sum * r_sum;
				r_sum += slep_ddot(t,t) + slep_ddot(s_t,s_t) - 2*slep_ddot(s_t,t);
				l_sum = slep_ddot(Av,Av);

				if( r_sum <= 1e-20){
					bFlag = 1;
					break;
				}
				if( l_sum <= r_sum * L)
					break;
				else
					L = SLEPMAX(2*L, l_sum / r_sum);
			}
			alphap = alpha;
			alpha = ( 1.0+ sqrt(4*alpha*alpha+1.0))/2.0;
			valueL->data.vctr[iterStep] = L;
			slep_dmcopy(X,XXp);
			slep_daXpbY(SlepNoTranspose, -1.0, Xp, 1.0, XXp);
			slep_dvcopy(Ax, Axy);
			slep_daxpby(-1.0, y, 1.0, Axy);

			funVal->data.vctr[iterStep] = slep_ddot(Axy,Axy)/2.0 + lambda * slep_dsum(t);

			if(bFlag)
				break;
			if( opts->tFlag == 0){
				if( iterStep >= 2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dfrbnrm(XXp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dfrbnrm(XXp) <= (opts->tol) * max(slep_dfrbnrm(Xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeMatrix(Xp);
		slepFreeMatrix(XXp);
		slepFreeMatrix(S);
		slepFreeMatrix(ATAs);
		slepFreeMatrix(G);
		slepFreeMatrix(U);
		slepFreeMatrix(V);
		slepFreeVector(Axp);
		slepFreeVector(As);
		slepFreeVector(Axy);
		slepFreeVector(Av);
		slepFreeVector(t);
		slepFreeVector(tp);
		slepFreeVector(s_t);
		slepFreeVector(v);
		funVal->dim = iterStep;
		valueL->dim = iterStep;

	}


	if( opts->mFlag==1 && opts->lFlag==1 && SLEPDoubleApproxEquals(opts->q,2.0) ){
		int bFlag = 0, iterStep;
		double L=1.0, gamma = 1.0;
		double alpha, alphap, beta,tao;
		slep_matrix *Xp, *XXp, *S, *ATAs, *G, *U, *V, *ATAx, *ATAxp, *Xnew;
		slep_vector *Axp, *As, *Axy, *Av, *Axnew;
		slep_vector *t, *tp, *s_t, *v, *tnew, *v_t;
		double l_sum , r_sum;

		Xp = slepCopyToNewMatrix(X);
		XXp = slepAllocDenseMatrix(X->m, X->n);
		Xnew = slepAllocDenseMatrix(X->m, X->n);
		S = slepAllocDenseMatrix(X->m, X->n);
		G = slepAllocDenseMatrix(X->m, X->n);
		V = slepAllocDenseMatrix(X->m, X->n);		
		U = slepAllocDenseMatrix(X->m, X->n);
		ATAs = slepAllocDenseMatrix(X->m, X->n);
		ATAx = slepAllocDenseMatrix(X->m, X->n);
		ATAxp = slepAllocDenseMatrix(X->m, X->n);
		Axp = slepCopyToNewVector(Ax);
		Axnew = slepCopyToNewVector(Ax);
		As = slepAllocDenseVector(Ax->dim);
		Av = slepAllocDenseVector(Ax->dim);
		Axy = slepAllocDenseVector(Ax->dim);

		v = slepAllocDenseVector(X->m);
		t = slepAllocDenseVector(X->m);
		tp = slepAllocDenseVector(X->m);
		tnew = slepAllocDenseVector(X->m);
		v_t = slepAllocDenseVector(X->m);
		s_t = slepAllocDenseVector(X->m);

		{
			slep_vector x__i=*t;
			x__i.dim = X->n;
			x__i.inc = X->m;
			for(i=0; i< X->m; ++i){
				x__i.data.vctr = &(X->data.mtrx[i]);
				tp->data.vctr[i] = t->data.vctr[i] = slep_dnrm2(&x__i);
			}
		}
		slep_zerom(XXp);

		slep_dvcopy(Ax,As);
		// ATAs
		{
			slep_matrix A_part = *A;
			slep_vector ATAs_i ;
			slep_vector As_i;
			ATAs_i.dim = A->n; 
			ATAs_i.inc = 1;
			ATAs_i.sparsity = SlepDense;
			As_i.inc = 1;
			As_i.sparsity = SlepDense;
			for(i=0; i<k; i++){
				A_part.data.mtrx = &(A->data.mtrx[ind[i]]);
				A_part.m = ind[i+1] - ind[i];
				ATAs_i.data.vctr = &(ATAs->data.mtrx[i*ATAs->ld]);
				As_i.data.vctr = &(As->data.vctr[ind[i]]);
				As_i.dim = ind[i+1] - ind[i];
				slep_daAxpby(SlepTranspose,1.0,&A_part, &As_i, 0.0, &ATAs_i);
			}
		}
		slep_dmcopy(ATAs,ATAx);

		for(iterStep = 0; iterStep < opts->maxIter ; ++iterStep){
			slep_dmcopy(ATAx, ATAxp);
			if(iterStep!=0){
				slep_dvcopy(Ax,As);
				// ATAs
				{
					slep_matrix A_part = *A;
					slep_vector ATAs_i ;
					slep_vector As_i;
					ATAs_i.dim = A->n; 
					ATAs_i.inc = 1;
					ATAs_i.sparsity = SlepDense;
					As_i.inc = 1;
					As_i.sparsity = SlepDense;
					for(i=0; i<k; i++){
						A_part.data.mtrx = &(A->data.mtrx[ind[i]]);
						A_part.m = ind[i+1] - ind[i];
						ATAs_i.data.vctr = &(ATAs->data.mtrx[i*ATAs->ld]);
						As_i.data.vctr = &(As->data.vctr[ind[i]]);
						As_i.dim = ind[i+1] - ind[i];
						slep_daAxpby(SlepTranspose,1.0,&A_part, &As_i, 0.0, &ATAs_i);
					}
				}
				slep_dmcopy(ATAs,ATAx);
			}

			for(;;){
				if(iterStep!=0){
					alpha = (-gamma + sqrt(gamma*gamma + 4.0*L*gamma))/(2.0*L);
					beta = (gamma - gamma * alphap) / (alphap * gamma + alphap * L * alpha);
					slep_dmcopy(X,S);
					slep_daXpbY(SlepNoTranspose, beta, XXp, 1.0, S);

					slep_dvcopy(t,s_t);
					slep_daxpby(-beta, tp, 1.0+beta, s_t);

					slep_dvcopy(Ax,As);
					slep_daxpby(-beta, Axp, 1.0+beta, As);

					slep_dmcopy(ATAx,ATAs);
					slep_daXpbY(SlepNoTranspose,-beta, ATAxp, 1.0+beta, ATAs);
				}else{
					alpha = (-1.0 + sqrt(5.0)) / 2.0;
					beta = 0.0;
					slep_dmcopy(X,S);
					slep_dvcopy(t,s_t);
					slep_dvcopy(Ax,As);
					slep_dmcopy(ATAx,ATAs);
				}

				// g=ATAs-ATy;
				slep_dmcopy(ATAs,G);
				slep_daXpbY(SlepNoTranspose,-1.0, ATy, 1.0, G);

				// u = s - g/L;
				slep_dmcopy(S,U);
				slep_daXpbY(SlepNoTranspose,-1.0/L, G, 1.0, U);

				// v = s_t - lambda / L;
				DMEMSET(v->data.vctr, -lambda/L,v->dim,i);
				slep_daxpby(1.0,s_t,1.0,v);

				ep21R(Xnew->data.mtrx, tnew->data.vctr, U->data.mtrx, v->data.vctr, X->m, X->n);

				//V=xnew-s; different V here :)
				slep_dmcopy(Xnew,V);
				slep_daXpbY(SlepNoTranspose, -1.0, S, 1.0, V);

				// v_t = tnew - s_t;
				slep_dvcopy(tnew,v_t);
				slep_daxpby(-1.0,s_t,1.0,v_t);

				// Axnew
				{
					slep_matrix A_i = *A;
					slep_vector Ax_i= *Ax, x_i;
					x_i.dim = X->m;
					x_i.inc = 1;
					x_i.sparsity = SlepDense;
					for(i=0; i < k ; i++){
						A_i.m = ind[i+1] - ind[i];
						Ax_i.dim = ind[i+1] - ind[i];
						A_i.data.mtrx = &(A->data.mtrx[ind[i]]);
						Ax_i.data.vctr = &(Axnew->data.vctr[ind[i]]);
						x_i.data.vctr = &(Xnew->data.mtrx[i*(Xnew->ld)]);
						slep_daAxpby(SlepNoTranspose,1.0, &A_i, &x_i, 0.0, &Ax_i);
					}
				}
				// Av = Axnew - As;
				slep_dvcopy(Axnew, Av);
				slep_daxpby( -1.0, As, 1.0, Av);
				r_sum = slep_dfrbnrm(V);
				r_sum = r_sum * r_sum;
				r_sum += slep_ddot(v_t,v_t);
				l_sum = slep_ddot(Av,Av);

				if( r_sum <= 1e-20){
					bFlag = 1;
					break;
				}
				if( l_sum <= r_sum * L)
					break;
				else
					L = SLEPMAX(2*L, l_sum / r_sum);
			}
			gamma = L*alpha *alpha;
			alphap = alpha;

			valueL->data.vctr[iterStep] = L;
			tao = L * r_sum / l_sum;
			if(tao >= 5.0)
				L = L * 0.8;

			slep_dmcopy(X,Xp);
			slep_dmcopy(Xnew,X);
			slep_dmcopy(X,XXp);
			slep_daXpbY(SlepNoTranspose, -1.0, Xp, 1.0, XXp);
			slep_dvcopy(Ax,Axp);
			slep_dvcopy(Axnew, Ax);
			slep_dvcopy(t,tp);
			slep_dvcopy(tnew,t);

			slep_dvcopy(Ax, Axy);
			slep_daxpby(-1.0, y, 1.0, Axy);

			funVal->data.vctr[iterStep] = slep_ddot(Axy,Axy)/2.0 + lambda * slep_dsum(t);

			if(bFlag)
				break;
			if( opts->tFlag == 0){
				if( iterStep >= 2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dfrbnrm(XXp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dfrbnrm(XXp) <= (opts->tol) * max(slep_dfrbnrm(Xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeMatrix(Xp);
		slepFreeMatrix(XXp);
		slepFreeMatrix(Xnew);
		slepFreeMatrix(S);
		slepFreeMatrix(ATAs);
		slepFreeMatrix(ATAx);
		slepFreeMatrix(ATAxp);
		slepFreeMatrix(G);
		slepFreeMatrix(U);
		slepFreeMatrix(V);
		slepFreeVector(Axp);
		slepFreeVector(As);
		slepFreeVector(Axy);
		slepFreeVector(Av);
		slepFreeVector(t);
		slepFreeVector(tp);
		slepFreeVector(s_t);
		slepFreeVector(v);
		slepFreeVector(v_t);
		slepFreeVector(tnew);
		slepFreeVector(Axnew);

		funVal->dim = iterStep;
		valueL->dim = iterStep;
	}
	slepFreeMatrix(ATy);
	slepFreeVector(Ax);
}